namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    
    /// <summary>
    /// Agent that compares the robot positioning estimate from the navigation pipeline with ground truth pose, as reported by camera pose estimation
    /// </summary>
    [DataContract]
    public class ShortNavRunVerifierAgent : SynchronizedConsumerProducerAgent<RobotPoseEstimationAgentMessage, SLAM2DAgentMessage, CompletionMessage>,
        ISubscriptionReceiver<CompletionMessage>
    {
        /// <summary>
        /// The maximum discrepancy between navigation and ground truth originating times.
        /// </summary>
        private const int DefaultSyncTolerance = 1000;

        /// <summary>
        /// The name of the agent producing the robot pose based on landmarks
        /// </summary>
        [DataMember(Name = "GroundTruthPoseProducer")]
        private readonly string groundTruthPoseProducer;

        /// <summary>
        /// The name of the agent that produces navigation poses
        /// </summary>
        [DataMember(Name = "NavigationPoseProducer")]
        private readonly string navigationPoseProducer;

        /// <summary>
        /// Active landmark ID used in the short recording
        /// </summary>
        [DataMember(Name = "LandmarkID")]
        private readonly int landmarkID;

        /// <summary>
        /// The name of the agent we use to update the landmark index.
        /// </summary>
        [DataMember(Name = "LandmarkPoseSinkName")]
        private readonly string landmarkPoseSinkAgentName = default(string); 

        /// <summary>
        /// We react to completion message from the gamepad (left bumper)
        /// </summary>
        [DataMember(Name = "CompletionEventProducer")]
        private readonly string completionEventAgentName = default(string);

        /// <summary>
        /// In case we don't have a configured landmark at the finish - we can set the final pose manually
        /// If this field is set - we will ignore the information coming from the ground truth system
        /// </summary>
        [DataMember(Name = "ManuallySetFinishPose")]
        private readonly Pose2D? manuallySetFinishPose;

        /// <summary>
        /// We need to send the landmark ID to this agent in order for Camera Pose Estimation agent to produce correct offsets.
        /// </summary>
        private IAgentHost landmarkPoseSinkAgent; 

        /// <summary>
        /// Camera Pose Estimation Input Forwarder used to pass the UI updates to the camera pose estimation agent.
        /// </summary>
        private IForwarder<CameraPoseEstimationInputMessage> landmarkPoseSinkForwarder;

        /// <summary>
        /// Last ground truth pose
        /// </summary>
        private Pose2D lastGroundTruthPose;

        /// <summary>
        /// Last localization pose
        /// </summary>
        private Pose2D lastLocalizationPose;

        /// <summary>
        /// Map origin, which we need to compute offset when finish pose is set manually
        /// </summary>
        private Pose2D mapStartPose;

        /// <summary>
        /// Initializes a new instance of the <see cref="ShortNavRunVerifierAgent" /> class.
        /// </summary>
        /// <param name="name">The name of the calculator agent</param>
        /// <param name="groundTruthPoseProducer">Ground Truth Pose Producer</param>
        /// <param name="navigationPoseProducer">Navigation Pose Producer</param>
        /// <param name="landmarkIDSinkAgentName">Landmark ID sink agent name</param>
        /// <param name="completionEventAgentName">Completion event agent name</param>
        /// <param name="targetLandmarkID">Target landmark ID</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        public ShortNavRunVerifierAgent(
            string name, 
            string groundTruthPoseProducer, 
            string navigationPoseProducer, 
            string landmarkIDSinkAgentName,
            string completionEventAgentName, 
            int targetLandmarkID,
            Pose2D? manuallySetFinishPose)
            : base(name, DefaultSyncTolerance, true)
        {
            this.groundTruthPoseProducer = groundTruthPoseProducer;
            this.navigationPoseProducer = navigationPoseProducer;
            this.landmarkPoseSinkAgentName = landmarkIDSinkAgentName;
            this.completionEventAgentName = completionEventAgentName;
            this.landmarkID = targetLandmarkID;
            this.manuallySetFinishPose = manuallySetFinishPose;
        }

        /// <summary>
        /// Gets or sets error delta
        /// </summary>
        public Pose2D DeltaError
        {
            get;
            set;
        }

        /// <summary>
        /// The name of the slam agent we measure
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.groundTruthPoseProducer; }
        }

        /// <summary>
        /// The name of the ground truth agent we compare with
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.navigationPoseProducer; }
        }

        /// <summary>
        /// Initialize the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            IAgentHost agent = locator.GetAgent(this.completionEventAgentName);
            agent.Subscribe<CompletionMessage>(this);

            this.landmarkPoseSinkAgent = locator.GetAgent(this.landmarkPoseSinkAgentName);
            this.landmarkPoseSinkForwarder = this.landmarkPoseSinkAgent.GetControlPort<CameraPoseEstimationInputMessage>();
        }

        /// <summary>
        /// Initializes the agent on activation. This acts as a constructor because the agent is activated/deactivated multiple times.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.landmarkPoseSinkForwarder.Post(new CameraPoseEstimationInputMessage(this.landmarkID));
        }

        /// <summary>
        /// Custom deactivate calls
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
        }

        /// <summary>
        /// Last RGB camera message is indication that we can compute the result
        /// </summary>
        /// <param name="completionMsg">The message</param>
        public void Receive(CompletionMessage completionMsg)
        {
            if (null != this.manuallySetFinishPose)
            {
                // Because SLAM reports poses in map start pose relative units, we have to covert that to map origin-relative values, which is
                // what we can easily get in the UI (either from our map visualizer or bitmap pixels)
                Pose2D mapOriginAdjustedFinishPose = new Pose2D(
                    this.manuallySetFinishPose.Value.X - this.mapStartPose.X, 
                    this.manuallySetFinishPose.Value.Y - this.mapStartPose.Y,
                    this.manuallySetFinishPose.Value.Heading - this.mapStartPose.Heading);

                this.DeltaError = mapOriginAdjustedFinishPose - this.lastLocalizationPose;
            }
            else
            {
                this.DeltaError = this.lastGroundTruthPose - this.lastLocalizationPose;
            }

            Console.WriteLine("-------------------------------------------- Results ready -----------------------------------------------------------");
            Console.WriteLine("--");
            Console.WriteLine("--");
            Console.WriteLine("Diff. between actual and computed robot pose: X:{0} Y:{1} Rot:{2}", this.DeltaError.X, this.DeltaError.Y, this.DeltaError.Heading);

            this.Publisher.Post(completionMsg);
        }

        /// <summary>
        /// Called when a pair of messages becomes available
        /// </summary>
        /// <param name="groundTruthPose">Ground Truth Pose</param>
        /// <param name="navPose">Navigation Pose</param>
        protected override void Receive(RobotPoseEstimationAgentMessage groundTruthPose, SLAM2DAgentMessage navPose)
        {
            if (groundTruthPose.RobotPose.HasValue)
            {
                this.lastGroundTruthPose = groundTruthPose.RobotPose.Value;
            }

            this.lastLocalizationPose = navPose.Location;

            this.mapStartPose = new Pose2D(
                navPose.Map.MapCellsToMeters(navPose.Map.MapStartPose.X),
                navPose.Map.MapCellsToMeters(navPose.Map.MapStartPose.Y),
                navPose.Map.MapStartPose.Heading);

            TraceOut.Info(TraceContexts.Nav, "GroundTruth:{0} SLAM Pose:{1} ", groundTruthPose.RobotPose.ToString(), navPose.Location.ToString());                        
        }
    }
}
